#include <fstream>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <TCanvas.h>
#include <TF1.h>
#include <TFile.h>
#include <TGraph.h>
#include <TH2F.h>
#include <TKey.h>
#include <TMath.h>
#include <TTree.h>
#include <TVector3.h>

void ge_dc_vs_beta(TCanvas *, std::string const &);
void turn(double, double, double, double, double *, double *);

void ge_dc_vs_beta(TCanvas *canvas, std::string const &filename)
{
  TFile file(filename.c_str());
  if (!file.IsOpen()) {
    throw std::runtime_error("Could not open file.");
  }
  TObject *obj = file.GetKey("is430")->ReadObj();
  if (!obj || 0 != strcmp("TTree", obj->ClassName())) {
    throw std::runtime_error("Missing tree 'is430'.");
  }
  TTree *tree = dynamic_cast<TTree *>(obj);

  std::ifstream angle_file("ToolsCalibration/cluster_angles.txt");
  if (!angle_file.is_open()) {
    throw std::runtime_error("Could not open angles file.");
  }
  double start_angles[8][4];
  for (int i = 0; 8 > i; ++i) {
    char line[80];
    angle_file.getline(line, 80);
    std::istringstream iss(line);
    for (int j = 0; 4 > j; ++j) {
      float f;
      iss >> f;
      start_angles[i][j] = f;
    }
  }
  angle_file.close();

  double th[8][24], ph[8][24];
  for (int clu = 0; 8 > clu; ++clu) {
    turn(start_angles[clu][0], start_angles[clu][1], start_angles[clu][2],
        start_angles[clu][3], th[clu], ph[clu]);
  }

  float ge_e;
  UShort_t ge_id;
  tree->SetBranchAddress("ge_e", &ge_e);
  tree->SetBranchAddress("ge_id", &ge_id);

  UChar_t fb_channel;
  float fb_gamma, fb_beta;
  float fb_x, fb_y, fb_z;
  tree->SetBranchAddress("fb_channel", &fb_channel);
  tree->SetBranchAddress("fb_beta", &fb_beta);
  tree->SetBranchAddress("fb_gamma", &fb_gamma);
  tree->SetBranchAddress("fb_x", &fb_x);
  tree->SetBranchAddress("fb_y", &fb_y);
  tree->SetBranchAddress("fb_z", &fb_z);

//  TH2F hist("hist", "ge_vs_ne_theta", 240, 900, 1140, 240, 900, 1140);
  TH1F hist0("hist0", "hist0", 240, 900, 1140);
  TH1F hist1("hist1", "hist1", 240, 900, 1140);

  float gamma = 2.85 / 939.0 + 1;
  float beta = sqrt(1 - 1 / (gamma * gamma));
  int map[] = {1, 2, 4, 5, 6, 7};
  for (int i = 0; tree->GetEntries() > i; ++i) {
    tree->GetEntry(i);
    int clu = ge_id / 100;
    int cry = (ge_id - 100 * clu) / 10;
    int seg = ge_id - 10 * cry - 100 * clu;
    assert(0 <= clu && 7 >= clu);
    assert(0 <= cry && 2 >= cry);
    assert(0 <= seg && 5 >= seg);
    float gtheta = th[clu][cry * 8 + map[seg]];
    float gphi = ph[clu][cry * 8 + map[seg]];
    float gx = sin(gphi) * sin(gtheta);
    float gy = cos(gphi) * sin(gtheta);
    float gz = cos(gtheta);
    float cos_theta = gx * fb_x + gy * fb_y + gz * fb_z;
//    float cos_theta = gz;
    float E0 = ge_e * gamma * (1 - beta * gz);
    float E1 = ge_e * gamma * (1 - beta * cos_theta);
//    hist.Fill(E0, E1);
    hist0.Fill(E0);
    hist1.Fill(E1);
  }
  canvas->cd(0);
//  hist.DrawCopy("zcol");
  hist0.DrawCopy();
  hist1.SetLineColor(2);
  hist1.DrawCopy("same");
}

void turn(double t, double p, double a, double dist, double *th, double *ph) 
{
  double const kDeg2Rad = TMath::Pi() / 180.0;
  t = t * kDeg2Rad;
  p = p * kDeg2Rad;
  a = a * kDeg2Rad;
  double const kCore2Crystal = 34.0;

  double center_x[3];
  double center_y[3];
  double center_z[3];

  // Core A.
  center_x[0] = kCore2Crystal;
  center_y[0] = 0;
  center_z[0] = dist;

  // Core B.
  center_x[1] = -kCore2Crystal * sin(30 * kDeg2Rad);
  center_y[1] = -kCore2Crystal * cos(30 * kDeg2Rad);
  center_z[1] = dist;

  // Core C.
  center_x[2] = -kCore2Crystal * sin(30 * kDeg2Rad);
  center_y[2] = kCore2Crystal * cos(30 * kDeg2Rad);
  center_z[2] = dist;

  double const kR = kCore2Crystal * 0.6;
  double const kRs = kR * sin(30.0 * kDeg2Rad);
  double const kRc = kR * cos(30.0 * kDeg2Rad);

  double x[24];
  double y[24];
  double z[24];

  // Crystal A.
  x[0] = center_x[0];        // A0 (core)
  y[0] = center_y[0];
  z[0] = center_z[0];
  x[1] = center_x[0] + kRs;  // A1
  y[1] = center_y[0] - kRc;
  z[1] = center_z[0];
  x[2] = center_x[0] - kRs;  // A2
  y[2] = center_y[0] - kRc;
  z[2] = center_z[0];
  x[3] = 0.0;                // empty DGF channel
  y[3] = 0.0;
  z[3] = 0.0;
  x[4] = center_x[0] + kR;   // A3
  y[4] = center_y[0] + 0.0;
  z[4] = center_z[0];
  x[5] = center_x[0] + kRs;  // A4
  y[5] = center_y[0] + kRc;
  z[5] = center_z[0];
  x[6] = center_x[0] - kR;   // A5
  y[6] = center_y[0] + 0.0;
  z[6] = center_z[0];
  x[7] = center_x[0] - kRs;  // A6
  y[7] = center_y[0] + kRc;
  z[7] = center_z[0];

  // Crystal B.
  x[8] = center_x[1];
  y[8] = center_y[1];
  z[8] = center_z[1];
  x[9] = center_x[1] - kR;
  y[9] = center_y[1] + 0.0;
  z[9] = center_z[1];
  x[10] = center_x[1] - kRs;
  y[10] = center_y[1] + kRc;
  z[10] = center_z[1];
  x[11] = 0.0;
  y[11] = 0.0;
  z[11] = 0.0;
  x[12] = center_x[1] - kRs;
  y[12] = center_y[1] - kRc;
  z[12] = center_z[1];
  x[13] = center_x[1] + kRs;
  y[13] = center_y[1] - kRc;
  z[13] = center_z[1];
  x[14] = center_x[1] + kRs;
  y[14] = center_y[1] + kRc;
  z[14] = center_z[1];
  x[15] = center_x[1] + kR;
  y[15] = center_y[1] + 0.0;
  z[15] = center_z[1];

  // Crystal C.
  x[16] = center_x[2];
  y[16] = center_y[2];
  z[16] = center_z[2];
  x[17] = center_x[2] + kRs;
  y[17] = center_y[2] + kRc;
  z[17] = center_z[2];
  x[18] = center_x[2] + kR;
  y[18] = center_y[2] + 0.0;
  z[18] = center_z[2];
  x[19] = 0.0;
  y[19] = 0.0;
  z[19] = 0.0;
  x[20] = center_x[2] - kRs;
  y[20] = center_y[2] + kRc;
  z[20] = center_z[2];
  x[21] = center_x[2] - kR;
  y[21] = center_y[2] + 0.0;
  z[21] = center_z[2];
  x[22] = center_x[2] + kRs;
  y[22] = center_y[2] - kRc;
  z[22] = center_z[2];
  x[23] = center_x[2] - kRs;
  y[23] = center_y[2] - kRc;
  z[23] = center_z[2];

  // matrix = m_phi * m_theta * m_alpha
  double const cp = cos(p);
  double const sp = sin(p);
  double const ct = cos(t);
  double const st = sin(t);
  double const ca = cos(a);
  double const sa = sin(a);
  // Column major.
  double matrix[9] = {
    ca * cp - ct * sa * sp, cp * ct * sa + ca * sp, -sa * st,
    -cp * sa - ca * ct * sp, ca * cp * ct - sa * sp, -ca * st,
    -sp * st, cp * st, ct
  };

  // Transform!
  for (int i = 0; 24 > i; ++i) {
    double xi = matrix[0] * x[i] + matrix[3] * y[i] + matrix[6] * z[i];
    double yi = matrix[1] * x[i] + matrix[4] * y[i] + matrix[7] * z[i];
    double zi = matrix[2] * x[i] + matrix[5] * y[i] + matrix[8] * z[i];
    th[i] = atan2(sqrt(xi * xi + yi * yi), zi);
    ph[i] = atan2(-xi, yi);
  }
}
